﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using NIP.ControllerInterface;
using System.Diagnostics;
using NIP.DynamicControl;
using NIP.DynamicControl.Constants;
using NIP.DynamicControl.InverseKinematics;
using System.IO;


namespace NIP.DynamicControl.Environ
{
    
    public class PathPlanning
    {
        
        private double[] _defaultPos = new double[USBC.TotalAxes] { 400, 400, 500, 0, 0, 0, 0, 0 };
        private PathPoint _goal;
        private PathPoint _currentPos;
        public PathPoint desiredPosTemp = new PathPoint();
        public int[] goalCell = new int[USBC.TotalAxes];
        //private Position testCurrentPos = new Position() { XYZd = new double[USBC.TotalAxes] { 400, -400, 400, 0, 0, 0, 0, 0 } };
        public List<int[]> fullGridSearchList = new List<int[]>();
        public List<int[]> partialGridSearchList = new List<int[]>();        
        public List<PathPoint> pathPoints = new List<PathPoint>();
        //public List<PathSection> path = new List<PathSection>();
        //public List<PathSection> currentPath = new List<PathSection>();
        public List<List<PathSection>> pathQueue= new List<List<PathSection>>();
        public List<PathSection> newPath = new List<PathSection>();
        public bool abortPath = false;
        public int pathIndex;
        public int pathQueueIndex;
        public const int maxIterations = 100;
        //public int searchListLength;
        public int totalGridCells = 0;
        public int openGridCells = 0;
        public const double stepLength = gridconst.GRIDRES;
        //public int[] XYZGoal { set; get; }
        public int[] XYZCurrent { set; get; }
        public int[, ,] gridType { set; get; }
        public double[, ,] grid { set; get; }
        private DateTime pathStartTime;
        private DateTime currentTime;
        private TimeSpan dt;
        //public const double DrawContourZOffset = 30.0;
        public bool InMotion = true;


        public event EventHandler<RobotStatusUpdateEventArgs> PathPlanningRobotStatusUpdated;
        public event EventHandler<Event.PathPlanningPositionUpdateEventArgs> PathPlanningRobotPositionStatusUpdated;


        public PathPlanning()
        {
            Console.WriteLine("Path Planning created");
            gridType = new int[gridconst.xcells, gridconst.ycells, gridconst.zcells];
            grid = new double[gridconst.xcells, gridconst.ycells, gridconst.zcells];
            //_currentPos = testCurrentPos;
            XYZCurrent = new int[USBC.TotalAxes];

            
            BuildGrid();

            BuildGridSearchList(fullGridSearchList);
            //RelaxGrid();
            //BuildPath();
            //PrintGrid();


        }

        public bool setInitialPath(PathPoint currentPos)
        {
            double[,] va = new double[2,3] { {0,0,0}, {0,0,0} };
            DateTime temp = DateTime.Now;
            PathSection tempSection = new PathSection(ControlCommands.NULL, 5, currentPos, currentPos, 0, 1, 'H');
            List<PathSection> tempPath = new List<PathSection>();
            tempPath.Add(tempSection);
            pathQueue.Add(tempPath);
            pathQueueIndex = 0;
            pathIndex = 0;
            pathStartTime = DateTime.Now;
            //currentPath = pathQueue[pathQueueIndex];
            return true;
        }

        

        public PathPoint getDesiredPos(double t_incr)
        {
            currentTime = DateTime.Now;
            currentTime.AddSeconds(t_incr);
            //Debug.Write("getDesiredPos: ");
            PathPoint returnPos = new PathPoint();
            dt = currentTime - pathStartTime;
            getPathSection(dt.TotalSeconds);
            //if (abortPath)
            //Debug.WriteLine("PP InMotion:" + InMotion);
            if (((dt.TotalSeconds - 0.5) > pathQueue[pathQueueIndex][pathIndex].getEndTime()))// && (!InMotion) )
            {
                var args = new RobotStatusUpdateEventArgs("");
                args.completedCommand = pathQueue[pathQueueIndex][pathIndex].getCommand();
                if (PathPlanningRobotStatusUpdated != null)
                {
                    //if (args.completedCommand == ControlCommands.TAKE_PHOTO)
                    //{
                    //    args.status = "Please smile at the camera now!";
                    //    Debug.WriteLine("TAKE PHOTO NOW");
                    //}
                    //if (args.completedCommand == ControlCommands.CONTOUR_BUILD)
                    //{
                    //    args.status = "Please be patient. You can't rush art!";
                    //}
                    if (args.completedCommand != ControlCommands.NULL)
                        PathPlanningRobotStatusUpdated(new object(), args);

                    //Debug.WriteLine("AFTER EVENT FIRED");

                   
                }
                if (((pathQueue.Count - 1) > pathQueueIndex) || (abortPath))
                {
                    //path = newPath;
                    //newPath = new List<PathSection>();
                    pathQueueIndex++;
                    pathIndex = 0;
                    //currentPath = pathQueue[pathQueueIndex]; 
                    abortPath = false;
                    pathStartTime = DateTime.Now;
                }
            }
                
            
            if ((pathIndex == pathQueue[pathQueueIndex].Count - 1) && (dt.TotalSeconds > pathQueue[pathQueueIndex][pathIndex].getEndTime() ))
            {
                returnPos = pathQueue[pathQueueIndex][pathIndex].getEndPos();
            }
            else
            {

                returnPos = pathQueue[pathQueueIndex][pathIndex].solveForJointAngles(dt.TotalSeconds);
                
                //if (pathtype == CONTOUR)
                //{
                //    returnPos = path[pathIndex].solveForPosition(dt.TotalSeconds);
                //    DynamicControl.InverseKinematics.Inv_K.solveMoveToJointAngles(returnPos);
                //}
            }
            if (returnPos.pos[2] < 0.0 || returnPos.pos[2] == 0)
            {
                //throw new Exception();
            }
            //Debug.Write("X:" + returnPos.pos[0] + " Y:" + returnPos.pos[1] + " Z:" + returnPos.pos[2]);
            //Debug.Write(" J0:" + returnPos.getJoint(0) + " J1:" + returnPos.getJoint(1) + " J2:" + returnPos.getJoint(2));
            //Debug.WriteLine(" J3:" + returnPos.getJoint(3) + " J4:" + returnPos.getJoint(4) + " W:" + returnPos.getPitch());
            var posStatus = new Event.PathPlanningPositionUpdateEventArgs(returnPos.pos);
            if (PathPlanningRobotPositionStatusUpdated != null)
            {
                PathPlanningRobotPositionStatusUpdated(new object(), posStatus);
            }
            return returnPos;
        }

        private void getPathSection(double pathTime)
        {
            while (pathIndex < (pathQueue[pathQueueIndex].Count - 1))
            {
                if (pathQueue[pathQueueIndex][pathIndex].getEndTime() > pathTime)
                {
                    break;
                }
                pathIndex++;
            }
        }

        public PathPoint goal
        {
            set
            {
                int[] XYZtemp = new int[USBC.TotalAxes];
                if (Inv_K.XYZisfeasible(value))
                {
                    if (_goal != null)
                    {
                        XYZtemp = gridconst.XYZtoGrid(_goal);
                        gridType[XYZtemp[0], XYZtemp[1], XYZtemp[2]] = 0;
                    }
                    _goal = value;
                    goalCell = gridconst.XYZtoGrid(_goal);
                    gridType[goalCell[0], goalCell[1], goalCell[2]] = -1;
                    grid[goalCell[0], goalCell[1], goalCell[2]] = gridconst.goalForce;
                    Console.WriteLine("Goal set to " + value.pos[0] + " " + value.pos[1] + " " + value.pos[2]);
                }
                else
                    Console.WriteLine("Invalid Goal position requested " + value.pos[0] + " " + value.pos[1] + " " + value.pos[2]);

            }
            get
            {
                return _goal;
            }
            

        }
        public void BuildGrid()
        {
            int xcell, ycell, zcell;
            totalGridCells = gridconst.xcells * gridconst.ycells * gridconst.zcells;
            for (xcell = 0; xcell < gridconst.xcells; xcell++)
            {
                for (ycell = 0; ycell < gridconst.ycells; ycell++)
                {
                    for (zcell = 0; zcell < gridconst.zcells; zcell++)
                    {
                        if ((xcell == 0) || (xcell == gridconst.xcells - 1) ||
                            (ycell == 0) || (ycell == gridconst.ycells - 1) ||
                            (zcell == 0) || (zcell == gridconst.zcells - 1))
                            gridType[xcell, ycell, zcell] = 1;
                        else if (Inv_K.Cellisfeasible(xcell, ycell, zcell))
                        {
                            gridType[xcell, ycell, zcell] = 0;
                            openGridCells++;
                        }
                        else
                            gridType[xcell, ycell, zcell] = 1;
                    }
                }
            }
        }
        public void BuildGridSearchList(List<int[]> searchList )
        {
            int xcell, ycell, zcell;
            bool onopenspace;
            int[] tempvec;
            searchList.Clear();
            for (ycell = 0; ycell < gridconst.ycells - 1; ycell++) 
            {
                for (zcell = 0; zcell < gridconst.zcells - 1; zcell++)
                {
                    onopenspace = false; 
                    tempvec = new int[4];
                    for (xcell = 0; xcell < gridconst.xcells - 1; xcell++)
                    {                        
                        if( (gridType[xcell,ycell,zcell] == 0) && (!onopenspace) )
                        {
                            onopenspace = true;
                            tempvec[0] = xcell;
                            tempvec[2] = ycell;
                            tempvec[3] = zcell;
                        }
                        else if( (gridType[xcell,ycell,zcell] == 1) && (onopenspace) )
                        {
                            onopenspace = false;
                            tempvec[1] = xcell-1;
                            searchList.Add(tempvec);
                            
                        }
                    }
                    if(onopenspace)
                    {
                        onopenspace = false;
                        tempvec[1] = xcell-1;
                        searchList.Add(tempvec);
                    }
                }
            }
            //searchListLength = searchList.Count;
        }
        public bool BuildPathPoints(PathPoint currentPos)
        {

            //Position currentPos = new Position();
            //currentPos = controllerObject.GetcurrentPosition();
            //Debug.WriteLine("Current pos:" + _currentPos.Joint[0] + " " + _currentPos.Joint[1] + " " + _currentPos.Joint[2] + " " + _currentPos.Joint[3] + " " + _currentPos.Joint[4]);

            double[] vel = new double[USBC.TotalAxes] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
            double[] acc = new double[USBC.TotalAxes] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
            double distToGoal, gradX, gradY, gradZ, gradMag;
            if (currentPos != null)
            {
                Console.Write("Planning Path from ");
                Console.Write("XYZ [" + currentPos.pos[0] + "," + currentPos.pos[1] + "," + currentPos.pos[2]);
                Console.WriteLine("] to [" + goal.pos[0] + "," + goal.pos[1] + "," + goal.pos[2] + "]");
                //Position tempPos = new Position() {XYZd = currentPos.XYZd };
                PathPoint tempPos = new PathPoint(currentPos);
                PathPoint goalPos = new PathPoint(goal);

                int[] Gridtemp = new int[USBC.TotalAxes];
                int pathsteps = 100;
                int step = 0;
                while (step < pathsteps)
                {
                    //Position nextPos = new Position { XYZd = tempPos.XYZd };
                    pathPoints.Add(tempPos);
                    tempPos = new PathPoint(pathPoints.Last());
                    //gridconst.XYZtoGrid(tempPos.pos);
                    distToGoal = Math.Sqrt(((goalPos.pos[0] - tempPos.pos[0]) * (goalPos.pos[0] - tempPos.pos[0]))
                                + ((goalPos.pos[1] - tempPos.pos[1]) * (goalPos.pos[1] - tempPos.pos[1]))
                                + ((goalPos.pos[2] - tempPos.pos[2]) * (goalPos.pos[2] - tempPos.pos[2])));
                    Debug.Write("Dist to goal:" + (int)distToGoal + " step: " + (int)stepLength + " ");
                    if (distToGoal < gridconst.GRIDRES)
                    {
                        Debug.WriteLine("***GOAL REACHED***");
                        pathPoints.Add(goalPos);
                        break;
                    }
                    Gridtemp = gridconst.XYZtoGrid(tempPos.pos);
                    gradX = grid[Gridtemp[0] - 1, Gridtemp[1], Gridtemp[2]] - grid[Gridtemp[0] + 1, Gridtemp[1], Gridtemp[2]];
                    gradY = grid[Gridtemp[0], Gridtemp[1] - 1, Gridtemp[2]] - grid[Gridtemp[0], Gridtemp[1] + 1, Gridtemp[2]];
                    gradZ = grid[Gridtemp[0], Gridtemp[1], Gridtemp[2] - 1] - grid[Gridtemp[0], Gridtemp[1], Gridtemp[2] + 1];
                    gradMag = Math.Sqrt((gradX * gradX) + (gradY * gradY) + (gradZ * gradZ));
                    //Debug.WriteLine("Step " + step + ": Grad[" + gradX + "," + gradY + "," + gradZ + "," + gradMag + "]");
                    tempPos.pos[0] += ((gradX / gradMag) * stepLength);
                    tempPos.pos[1] += ((gradY / gradMag) * stepLength);
                    tempPos.pos[2] += ((gradZ / gradMag) * stepLength);
                    //robotObject.Inv_KObject.solveMoveToJointAngles;
                    //Debug.WriteLine("Step " + step + ": [" + tempPos.XYZd[0] + "," + tempPos.XYZd[1] + "," + tempPos.XYZd[2] + "]");
                    step++;
                    //nextPos = null;
                }

                // dejitter path
                for (int i = 1; i < pathPoints.Count - 1; i++)
                {
                    gradX = pathPoints[i + 1].pos[0] - pathPoints[i - 1].pos[0];
                    gradY = pathPoints[i + 1].pos[1] - pathPoints[i - 1].pos[1];
                    gradZ = pathPoints[i + 1].pos[2] - pathPoints[i - 1].pos[2];
                    gradMag = Math.Sqrt((gradX * gradX) + (gradY * gradY) + (gradZ * gradZ));
                    if (gradMag < stepLength)
                    {
                        Debug.WriteLine("X i + 1:i - 1| " + pathPoints[i + 1].pos[0] + ":" + pathPoints[i - 1].pos[0]);
                        Debug.WriteLine("Dejitter: Point removed " + i + "| GradMag:step " + gradMag + ":" + stepLength); 
                        pathPoints.RemoveAt(i);
                        i -= 1;
                    }
                }
#if DEBUG
                Debug.WriteLine("Path after dejitter, before pitch stepping");
                for (int i = 0; i < pathPoints.Count; i++)
                {
                    Debug.WriteLine(String.Format("PP{0,3:N}|X{1,4:N2} Y{2,4:N2} Z{3,5:N2}|J0:{4,4:N1} J1:{5,4:N1} J2:{6,4:N1} J3:{7,4:N1} J4:{8,4:N1}|", i, pathPoints[i].pos[0], pathPoints[i].pos[1], pathPoints[i].pos[2], pathPoints[i].getJoint(0), pathPoints[i].getJoint(1), pathPoints[i].getJoint(2), pathPoints[i].getJoint(3), pathPoints[i].getJoint(4)));
                }
#endif

                double rollStep = (pathPoints[pathPoints.Count - 1].getJoint(4) - pathPoints[0].getJoint(4)) / (pathPoints.Count - 1);
                double pitchStep = (pathPoints[pathPoints.Count - 1].getJoint(3) - pathPoints[0].getJoint(3)) / (pathPoints.Count - 1);
                Debug.WriteLine("J3| Final:" + pathPoints[pathPoints.Count - 1].getJoint(3) + " initial:" + pathPoints[0].getJoint(3) + " points:" + pathPoints.Count + " pitchstep:" + pitchStep);
                Debug.WriteLine("J4| Final:" + pathPoints[pathPoints.Count - 1].getJoint(4) + " initial:" + pathPoints[0].getJoint(4) + " points:" + pathPoints.Count + " pitchstep:" + rollStep);
                for (int i = 1; i < pathPoints.Count - 1; i++)
                {
                    gradX = pathPoints[i + 1].pos[0] - pathPoints[i - 1].pos[0];
                    gradY = pathPoints[i + 1].pos[1] - pathPoints[i - 1].pos[1];
                    gradZ = pathPoints[i + 1].pos[2] - pathPoints[i - 1].pos[2];
                    gradMag = Math.Sqrt((gradX * gradX) + (gradY * gradY) + (gradZ * gradZ));
                    pathPoints[i].vel[0] = ((gradX / gradMag) * gridconst.pathVel);
                    pathPoints[i].vel[1] = ((gradY / gradMag) * gridconst.pathVel);
                    pathPoints[i].vel[2] = ((gradZ / gradMag) * gridconst.pathVel);
                    pathPoints[i].setJoint(3, pathPoints[i - 1].getJoint(3) + pitchStep); 
                    pathPoints[i].setJoint(4,pathPoints[i - 1].getJoint(4) + rollStep);
                    NIP.DynamicControl.InverseKinematics.Inv_K.solveMoveToJointAngles(pathPoints[i]);
                }
#if DEBUG
                Debug.WriteLine("Path after pitch stepping");
                for (int i = 0; i < pathPoints.Count; i++)
                {
                    Debug.WriteLine(String.Format("PP{0,3:N}|X{1,4:N2} Y{2,4:N2} Z{3,5:N2}|J0:{4,4:N1} J1:{5,4:N1} J2:{6,4:N1} J3:{7,4:N1} J4:{8,4:N1}|", i, pathPoints[i].pos[0], pathPoints[i].pos[1], pathPoints[i].pos[2], pathPoints[i].getJoint(0), pathPoints[i].getJoint(1), pathPoints[i].getJoint(2), pathPoints[i].getJoint(3), pathPoints[i].getJoint(4)));
                }
#endif


            }
            else
            {
                Debug.WriteLine("Null Current Position");
                return false;
            }
            return true;
        }                   
        public void BuildPathSections()
        {
            Debug.WriteLine("Planning Path sections");
            int pathPoint = pathPoints.Count;
            PathSection tempSection;
            List<PathSection> tempPath = new List<PathSection>();
            //DateTime time = DateTime.Now;
            //double planningdelay = 0.5;
            double stepDuration = 1.0;
            //time.AddSeconds(planningdelay);
            double pathTime = 0.0;
            double[,] va = new double[2,3] { {0,0,0}, {0,0,0} };
            for (int point = 0; point < pathPoints.Count - 2; point++)
            {
                //PathSection( double[,] vaInitial, double[,] vaFinal)
                tempSection = new PathSection(ControlCommands.NULL, 4, pathPoints[point], pathPoints[point + 1], pathTime, stepDuration, va, va, 'C');
                va[0, 0] += 1.0;
                va[0, 1] += 1.0;
                va[0, 2] += 1.0;
                va[1, 0] += 2.0;
                va[1, 1] += 2.0;
                va[0, 2] += 1.0;
                tempPath.Add(tempSection);
                //time.AddSeconds(stepDuration);
                pathTime += stepDuration;

                
                
            }
            pathQueue.Add(tempPath);
            
#if DEBUG
            Debug.WriteLine("Path sections");
            Debug.Write("|Sct|Time|");
            for(int j = 0; j < 5; j++)
                Debug.Write(String.Format("          J{0}          |",j));
            Debug.WriteLine("");
            Debug.Write("|        |");
            for (int j = 0; j < 5; j++)
            {
                Debug.Write(String.Format("  Pos     Vel     Acc |"));
                //Debug.Write(String.Format("-100.0 -1000.0 -1000.0|"));
            }
            Debug.WriteLine("");
            double jointPos, jointVel, jointAcc;
            int timeslices = 10;
            for (int s = 0; s < tempPath.Count; s++)
            {
                //Debug.WriteLine(String.Format("PP{0,3:N}|X{1,4:N2} Y{2,4:N2} Z{3,5:N2}|J0:{4,4:N1} J1:{5,4:N1} J2:{6,4:N1} J3:{7,4:N1} J4:{8,4:N1}|", i, pathPoints[i].pos[0], pathPoints[i].pos[1], pathPoints[i].pos[2], pathPoints[i].joint[0], pathPoints[i].joint[1], pathPoints[i].joint[2], pathPoints[i].joint[3], pathPoints[i].joint[4]));
                
                //path[p].getDuration();
                for(int t = 0; t < timeslices; t++)
                {
                    Debug.Write(String.Format("|{0,3:N0}|{1,4:N0}|", s, (s*100)+(t*10)));
                    for(int j = 0; j < 5; j++)
                    {
                        //solve joint pos, vel and acc at small time steps here
                        jointPos = 0.0;
                        jointVel = 0.0;
                        jointAcc = 0.0;
                        Debug.Write(String.Format("{0,6:N1} {1,7:N1} {2,7:N1}|", jointPos, jointVel, jointAcc));
                        //{1,4:N},{2,4:N},{3,4:N} - {4,4:N},{5,4:N},{6,4:N}|
                    }
                    Debug.WriteLine("");
                }
            }
#endif



        }
        public void BuildContourPath(List<double[]> contour)
        {
            Debug.WriteLine("Planning Contour");
            
            List<PathSection> tempPath = new List<PathSection>();
            PathSection tempSection;
            PathPoint startPos=  new PathPoint();
            PathPoint endPos = new PathPoint();
            double totalPathTime = 0.0;
            double sectionDuration = 0.3;
            int contourLength = contour.Count;
            
            startPos.pos[0] = gridconst.imgcenterX + contour[0][1];
            startPos.pos[1] = gridconst.imgcenterY + contour[0][0];
            //! Z changed by HL added Offset in Z
            startPos.pos[2] = gridconst.imgcenterZ;// +gridconst.DrawContourZOffset;
            startPos.WristPitch = -90.0;
            NIP.DynamicControl.InverseKinematics.Inv_K.solveMoveToJointAngles(startPos);

            Debug.WriteLine("point" + 0 + " XYZ " + startPos.pos[0] + ":" + startPos.pos[1] + ":" + startPos.pos[2]);

            //! Note to Mark: (HL) should this be < (contourLength -1) to cover index: (contourlength-2) as well?
            //! HL made change
            for (int cpoint = 1; cpoint < (contourLength - 1); cpoint++)
            {
                endPos.pos[0] = gridconst.imgcenterX + contour[cpoint][1];
                endPos.pos[1] = gridconst.imgcenterY + contour[cpoint][0];
                endPos.pos[2] = gridconst.imgcenterZ;
                endPos.WristPitch = -90.0;
                NIP.DynamicControl.InverseKinematics.Inv_K.solveMoveToJointAngles(endPos);
                tempSection = new PathSection(ControlCommands.NULL, 5, startPos, endPos, totalPathTime, sectionDuration, 'C');
                tempPath.Add(tempSection);
                totalPathTime += sectionDuration;
                startPos = endPos;
                endPos = new PathPoint();
                //newPath = path;
                
            }
            endPos.pos[0] = gridconst.imgcenterX + contour[contourLength - 1][1];
            endPos.pos[1] = gridconst.imgcenterY + contour[contourLength - 1][0];
            endPos.pos[2] = gridconst.imgcenterZ;// +gridconst.DrawContourZOffset;
            endPos.WristPitch = -90.0;
            NIP.DynamicControl.InverseKinematics.Inv_K.solveMoveToJointAngles(endPos);
            tempSection = new PathSection(ControlCommands.CONTOUR_BUILD, 5, startPos, endPos, totalPathTime, sectionDuration, 'C');
            tempPath.Add(tempSection);
            pathQueue.Add(tempPath);

            //abortPath = true;
        }

        
        
        public void BuildPathSection(ControlCommands command, PathPoint currentPos)
        {
            Debug.WriteLine("Planning Path section");
            //int pathPoint = pathPoints.Count;
            List<PathSection> tempPath = new List<PathSection>();
            PathSection tempSection;
            DateTime startTime = DateTime.Now;
            double stepDuration = 1.0;
            startTime.AddSeconds(gridconst.planningDelay);
            double[] jointDelta = new double[5];
            double[] xyzDelta = new double[3];
            PathPoint goalPos = new PathPoint(goal);
            for (int j = 0; j < 5; j++)
            {
                jointDelta[j] = goal.getJoint(j) - currentPos.getJoint(j);
            }
            for (int c = 0; c < 3; c++)
            {
                xyzDelta[c] = goal.pos[c] - currentPos.pos[c];
            }
            double xyzMag = Math.Sqrt((xyzDelta[0] * xyzDelta[0]) + (xyzDelta[1] * xyzDelta[1]) + (xyzDelta[2] * xyzDelta[2]));
            DateTime endTime = startTime;
            double pathDuration = (xyzMag / gridconst.pathVel);
            endTime.AddSeconds(pathDuration);
           
            tempSection = new PathSection(command, 5, currentPos, goalPos, 0.0, pathDuration, 'C');
            tempPath.Add(tempSection);
            //newPath = path;
            pathQueue.Add(tempPath);
            //abortPath = true;

        }


        public void RelaxGrid()
        {
            BuildGridSearchList(partialGridSearchList);
            int iterations = 0;
            int i, xcell, ycell, zcell;
            double gridaverage = 1000;
            double oldcellval = 0;
            while (Math.Abs(gridaverage) > gridconst.convergence)
            {
                gridaverage = 0;
                for (i = 0; i < partialGridSearchList.Count; i++)
                {
                    ycell = partialGridSearchList[i][2];
                    zcell = partialGridSearchList[i][3];
                    for (xcell = partialGridSearchList[i][0]; xcell <= partialGridSearchList[i][1]; xcell++)
                    {
                        
                        if ((xcell != goalCell[0]) || (ycell != goalCell[1]) || (zcell != goalCell[2]))
                        {
                            oldcellval = grid[xcell, ycell, zcell];
                            grid[xcell, ycell, zcell] = (grid[xcell + 1, ycell, zcell] +
                                                        grid[xcell - 1, ycell, zcell] +
                                                        grid[xcell, ycell + 1, zcell] +
                                                        grid[xcell, ycell - 1, zcell] +
                                                        grid[xcell, ycell, zcell + 1] +
                                                        grid[xcell, ycell, zcell - 1]) / 6.0;
                            gridaverage += (grid[xcell, ycell, zcell] - oldcellval);
                        }
                    }                  
                }
                //grid[goalCell[0], goalCell[1], goalCell[2]] = gridconst.goalForce;
                gridaverage /= openGridCells;
                iterations++;
            }
        }
        public void PrintGrid()
        {
            int xcell, ycell, zcell;

            for (zcell = 0; zcell < gridconst.zcells; zcell++)
            {
                Debug.WriteLine("z " + (zcell + 1) + " of " + (gridconst.zcells + 1) + ": GRIDRES " + gridconst.GRIDRES);
                Debug.Write("  ");
                for (ycell = 0; ycell < gridconst.ycells; ycell++)
                    Debug.Write(" " + ycell%10);
                Debug.WriteLine("");
                for (xcell = 0; xcell < gridconst.xcells; xcell++)
                {
                    if (xcell < 10)
                        Debug.Write(" ");
                    Debug.Write(xcell);
                    for (ycell = 0; ycell < gridconst.ycells; ycell++)
                    {
                        if (zcell == 0 || zcell == gridconst.zcells - 1)
                            Debug.Write(" *");
                        else if (xcell == 0 || xcell == gridconst.xcells - 1)
                            Debug.Write(" *");
                        else if (ycell == 0 || ycell == gridconst.ycells - 1)
                            Debug.Write(" *");
                        else if (gridType[xcell, ycell, zcell] == 0)
                            Debug.Write(" " + gridType[xcell, ycell, zcell]);
                        else
                            Debug.Write("  ");

                        
                    }
                    Debug.WriteLine("");
                }
            }
            Debug.WriteLine("");
            Debug.WriteLine("Grid cells X:" + gridconst.xcells + " Y:" + gridconst.ycells + " Z:" + gridconst.zcells);
            Debug.WriteLine("total grid cells: " + totalGridCells);
            Debug.WriteLine("feasible grid cells: " + openGridCells);
            Debug.WriteLine((openGridCells * 100.0 / totalGridCells) + "%");

        }
                            
        
    }
    
     
    
}
